from geometry_msgs.msg import PoseStamped  
from nav2_simple_commander.robot_navigator import BasicNavigator  
import rclpy  

def main():  
    rclpy.init()  
    nav = BasicNavigator()  
    init_pose = PoseStamped()  
    
    # 设置初始位姿的帧和时间戳  
    init_pose.header.frame_id = "map"  
    init_pose.header.stamp = nav.get_clock().now().to_msg()  
    
    # 正确设置位置和方向属性  
    init_pose.pose.position.x = 0.0  
    init_pose.pose.position.y = 0.0  
    init_pose.pose.position.z = 0.0  # 添加 z 坐标（如果需要）  
    
    init_pose.pose.orientation.x = 0.0  
    init_pose.pose.orientation.y = 0.0  # 修正为正确的 orientation 属性  
    init_pose.pose.orientation.z = 0.0  # 添加任意需要的值  
    init_pose.pose.orientation.w = 1.0  # 这是四元数的 w 分量，一般设置为 1.0 以表示无旋转  

    # 设置初始位姿  
    nav.setInitialPose(init_pose)  

    # 等待导航系统可用  
    nav.waitUntilNav2Active()  
    
    # 进入循环，保持程序运行  
    rclpy.spin(nav)  
    
    # 关闭 rclpy  
    rclpy.shutdown()  

# if __name__ == "__main__":  
#     main()